/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-09-04 09:57:24
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-09-26 19:39:46
 */

#ifndef  __ROBOTSERVOFW_H__
#define  __ROBOTSERVOFW_H__

#include "main.h"
#include <stdlib.h>

#define ServoNumber     6
#define ServoID_MAX     254
#define ServoPWM_MIN    500
#define ServoPWM_MAX    2500
#define ServoTIME_MIN   0
#define ServoTIME_MAX   9999
// 舵机当前角度模式：270度
#define ServoModeAngle  270
// 舵机每度的PWM值
#define ServoOneAngle   ((ServoPWM_MAX - ServoPWM_MIN) / (double)ServoModeAngle)

#define Clamp_ID        5
#define Clamp_Min       1200
#define Clamp_Max       2000

#define SERVO_ENABLE(state)     HAL_GPIO_WritePin(SERVO_POWER_GPIO_Port, SERVO_POWER_Pin, \
                                    (state? GPIO_PIN_SET : GPIO_PIN_RESET))

// 舵机结构体
typedef struct
{
    uint8_t id;             // 舵机ID
    uint16_t pwm;           // 舵机PWM值
    uint16_t time;          // 舵机执行时间
}ServoTypeDef;

extern uint8_t ServoCmd[256];
extern ServoTypeDef ServoOnlyOne;
extern ServoTypeDef ServoAll[ServoNumber];
extern int ServoBias[ServoNumber];

void Control_Servo_OnlyOne(uint8_t *cmd, ServoTypeDef *servo);
void Control_Servo_multi(uint8_t *cmd, ServoTypeDef *servoAll);
void Control_ServoReset(uint8_t *cmd);
void Read_PositionAll(UART_HandleTypeDef *huart);
void Control_Servo_PULR(uint8_t *cmd, uint8_t id, uint8_t state);
void Control_Servo_ALLPULR(uint8_t *cmd, uint8_t state);
double Servo_PWM2Angle(uint32_t pwm);

#endif  /* __ROBOTSERVOFW_H__ */

